#include "Model/Model.h"
#include "View/View.h"
#include "Controller/Controller.h"
#define USERS 15


int main(int argc, char **argv) {
	initOpenNIConfiguration();
	BulletConfiguration* bulletConfig = new BulletConfiguration();

//	bool usersSkeletonLoaded[USERS];
//	for(int i=0; i<USERS; i++)
//		usersSkeletonLoaded[i] = false;
//	CharacterSkeleton** skeletons = new CharacterSkeleton*[USERS];
//	printf("Ready\n");
	{
		//create a dynamic rigidbody

		btCollisionShape* colShape = new btBoxShape(btVector3(10,10,10));
		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
		bulletConfig->getCollisionShapes().push_back(colShape);

		/// Create Dynamic Objects
		btTransform startTransform;
		startTransform.setIdentity();

		btScalar	mass(1.f);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0,0,0);
		if (isDynamic)
			colShape->calculateLocalInertia(mass,localInertia);

		startTransform.setOrigin(btVector3(100,100,800));

		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);

		bulletConfig->getDynamicsWorld()->addRigidBody(body);
	}

	{
		//create a dynamic rigidbody

		btCollisionShape* colShape = new btBoxShape(btVector3(10,10,10));
		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
		bulletConfig->getCollisionShapes().push_back(colShape);

		/// Create Dynamic Objects
		btTransform startTransform;
		startTransform.setIdentity();

		btScalar	mass(1.f);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0,0,0);
		if (isDynamic)
			colShape->calculateLocalInertia(mass,localInertia);

		startTransform.setOrigin(btVector3(0,140,800));

		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);

		bulletConfig->getDynamicsWorld()->addRigidBody(body);
	}

	{
		//create a dynamic rigidbody

		btCollisionShape* colShape = new btBoxShape(btVector3(10,10,10));
		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
		bulletConfig->getCollisionShapes().push_back(colShape);

		/// Create Dynamic Objects
		btTransform startTransform;
		startTransform.setIdentity();

		btScalar	mass(1.f);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0,0,0);
		if (isDynamic)
			colShape->calculateLocalInertia(mass,localInertia);

		startTransform.setOrigin(btVector3(-100,100,800));

		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);

		bulletConfig->getDynamicsWorld()->addRigidBody(body);
	}

//	int k = 0;
	initGlutConfiguration(USERS, bulletConfig,&argc,argv);
	glutMainLoop();
//	while (true)
//	{
//		// Update to next frame
//		nRetVal = context.WaitAndUpdateAll();
//		checkErrorCode (nRetVal, "WaitAndUpdateAll");
//
//		// Extract head position of each tracked user
//		XnUserID aUsers[USERS];
//		XnUInt16 nUsers = USERS;
//
//		g_UserGenerator.GetUsers(aUsers, nUsers);
//		for (int i = 0; i < nUsers; ++i)
//		{
//			if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i])) {
//				if(!usersSkeletonLoaded[i]) {
//					skeletons[i] = new CharacterSkeleton(g_UserGenerator, aUsers[i], bulletConfig);
//					usersSkeletonLoaded[i] = true;
//				}
//				skeletons[i]->update();
//				if(k%60 == 0) {
//					XnSkeletonJointPosition lHand;
//					g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HAND, lHand);
//					printf("%d: (%f,%f,%f) [%f]\n", aUsers[i],lHand.position.X,
//							lHand.position.Y, lHand.position.Z,lHand.fConfidence);
//				}
//
//			}
//		}
//
//		bulletConfig->getDynamicsWorld()->stepSimulation(1.f/30.f,10);
//		//print positions of all object
//		if(k++%60 == 0) {
//			for (int j=bulletConfig->getDynamicsWorld()->getNumCollisionObjects()-1; j>=0 ;j--)
//			{
//				btCollisionObject* obj = bulletConfig->getDynamicsWorld()->getCollisionObjectArray()[j];
//				btRigidBody* body = btRigidBody::upcast(obj);
//				if (body && body->getMotionState()){
//					btTransform trans;
//					body->getMotionState()->getWorldTransform(trans);
//					printf("world pos = %f,%f,%f\n",float(trans.getOrigin().getX()),float(trans.getOrigin().getY()),float(trans.getOrigin().getZ()));
//					printf("\t%d\n",(int) body);
//				}
//			}
//		}
//	}
	// Clean up
	delete bulletConfig;
	shutdownOpenNIConfiguration();
	return 0;
}
